/* ######################################################################### */
/** \file ObjectIdentification.hpp
 * \brief	This file contains the function that
 *
 *
 * PROJECT:   DATMO ROS NODE
 *
 * Copyright (c) 2011 CAR - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2011-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Gonzalo RODRIGUEZ         2011-06-23 \n
 *      First .
 *
 ######################################################################### */


#include <math.h>
#include <set>
/* ------------------------------------------------------------------------- */
/**	\namespace DATMO
 *
 *  \brief	This is the namespace of the DATMO ROS NODE
 */
/* ------------------------------------------------------------------------- */

namespace DATMO
{
/* ------------------------------------------------------------------------- */
/**	\class ObjecIdentification
 *
 *  \brief  This is the class of Objects2Real function.
 *
 *
 */
/* ------------------------------------------------------------------------- */
class cl_ObjectIdentification
{
public:
	/* ------------------------------------------------------------------------- */
	/**	\fn public static void ObjecIdentification()
	 *
	 *  \brief
	 *
	 *  \param[in]
	 *  \param[in]
	 *
	 *  \param[out] bool
	 *
	 */
	/* ------------------------------------------------------------------------- */
	static cl_DynamicObjects2 ObjectIdentification(cl_DynamicObjects2 str_RobotObjectList_act,
							cl_DynamicObjects2 str_RobotObjectList,
							ros::Time ActTime, int RobotID)
	{
		DynObjConst str_DynObjConst = DynObjConst();
		float maxIDrobot = str_RobotObjectList.maxID;
		int n_list = -1;
		int n_act = -1;
		int n_add = 0;
		bool first = true;
		float distance;
		int i_aux = 0;
		int max_id = 0;
		float distance_tol = str_DynObjConst.f_object_distance_tol;
		float t_min_detect = str_DynObjConst.t_min_detect;
		float dist_min = 0;
		float pos1[3];
		float pos0[3];
		double vx, vy, vz;

		int n_eliminate;
		float inc_time = 0;

		ros::Time auxTime;
//		DynamicObjectListSet::iterator it;
//		DynamicObjectListSet::iterator it2;

		//*Identify objects from DynamicObjectsList.List
		// * with the ones defined in DynamicObjectsAct

		//Calculate the number of elements in the dynamic object list and in the actual object list
		n_list = str_RobotObjectList.List.size();
		n_act = str_RobotObjectList_act.List.size();
		if (n_list == 0 && n_act>0)
		{
			//str_RobotObjectList.DynamicObjects = new cl_DynamicObjects((ushort)n_act);
			str_RobotObjectList = str_RobotObjectList_act;
		}
		else if (n_act > 0 && n_list > 0)
		{
			n_eliminate = 0;

			double pred_pos[n_list][3];
			double act_pos[n_act][3];
			float pairs[n_list * n_act][ 3]; //ind_pred, ind_act,distance //all the possible pairs
			float obj_pairs[(int)fmin(n_list, n_act)][3]; //ind_pred,ind_act,distance //selected pairs

			int obj_act_added[n_act];
			//calculate the distance between the pairs
			//int i= 0;
			//for(it = str_RobotObjectList.List.begin();it!=str_RobotObjectList.List.end();it++)
			for(int i = 0;i<(int)str_RobotObjectList.List.size();i++)
			{
//				pos0[0] = *it->Pos3D.f_x;
//				pos0[1] = *it->Pos3D.f_z;
//				pos0[2] = *it->Pos3D.f_z;
//				vx = *it->Vel3D.d_vx;
//				vy = *it->Vel3D.d_vy;
//				vz = *it->Vel3D.d_vz;
				pos0[0] = str_RobotObjectList.List[i].Pos3D.f_x;
				pos0[1] = str_RobotObjectList.List[i].Pos3D.f_y;
				pos0[2] = str_RobotObjectList.List[i].Pos3D.f_z;
				vx = str_RobotObjectList.List[i].Vel3D.d_vx;
				vy = str_RobotObjectList.List[i].Vel3D.d_vy;
				vz = str_RobotObjectList.List[i].Vel3D.d_vz;
			//	auxTime = *it->r_DetectionTime;
				auxTime = str_RobotObjectList.List[i].r_DetectionTime;
				pred_pos[i][ 0] = pos0[0] + vx * (ActTime.sec - auxTime.sec);
				pred_pos[i][ 1] = pos0[1] + vy * (ActTime.sec - auxTime.sec);
				pred_pos[i][ 2] = pos0[2] + vz * (ActTime.sec - auxTime.sec);
				//j = 0;
				//for (it2 = str_RobotObjectList_act.List.begin();it2!=str_RobotObjectList_act.List.end();it2++)
				for(int j = 0;j<(int)str_RobotObjectList_act.List.size();j++)
				{
					if (first)
					{
						obj_act_added[j] = 0; // all the objects marked as not added. later mark the one added with 1
//						act_pos[j, 0] = (float)*it2->Pos3D.f_x;
//						act_pos[j, 1] = (float)*it2->Pos3D.f_y;
//						act_pos[j, 2] = (float)*it2->Pos3D.f_z;
						act_pos[j][0] = str_RobotObjectList_act.List[j].Pos3D.f_x;
						act_pos[j][1] = str_RobotObjectList_act.List[j].Pos3D.f_y;
						act_pos[j][2] = str_RobotObjectList_act.List[j].Pos3D.f_z;
					}
					distance = (float)sqrt((double)(pow((double)(act_pos[j][ 0] - pred_pos[i][ 0]), 2) +
							pow((double)(act_pos[j][ 2] - pred_pos[i][ 2]), 2)));
					pairs[j + i * n_act][ 0] = i;//*it->f_Identification;
					pairs[j + i * n_act][ 1] = j;//*it2->f_Identification;
					pairs[j + i * n_act][ 2] = distance;
					j++;
				}
				first = false;
				i++;
			}
			//Order and select pairs
			dist_min = 100000;
			while (i_aux < fmin(n_list, n_act))
			{
				for (int i = 0; i < (n_act * n_list); i++)
				{
					if (pairs[i][ 2] < dist_min && pairs[i][2]>0)
					{
						obj_pairs[i_aux][ 0] = pairs[i][ 0];
						obj_pairs[i_aux][ 1] = pairs[i][ 1];
						obj_pairs[i_aux][ 2] = pairs[i][ 2];
						dist_min = pairs[i][ 2];
					}
				}
				for (int i = 0; i < (n_act * n_list); i++)
				{
					if (pairs[i][ 0] == obj_pairs[i_aux][ 0])
						pairs[i][ 2] = -1;
					if (pairs[i][ 1] == obj_pairs[i_aux][ 1])
						pairs[i][ 2] = -1;
				}
				dist_min = 100000;
				i_aux++;

			}
			//Check each pair to see if distance its acceptable and to predict new position

			for (int i = 0; i < fmin(n_act, n_list); i++)
			{
				if (obj_pairs[i][ 2] < distance_tol)
				{
					obj_act_added[(int)obj_pairs[i][1]] = 1;
//					pos1[0] = *it2->Pos3D.f_x;
//					pos1[1] = *it2->Pos3D.f_y;
//					pos1[2] = *it2->Pos3D.f_z;
					pos1[0] = str_RobotObjectList_act.List[(int)obj_pairs[i][ 1]].Pos3D.f_x;
					pos1[1] = str_RobotObjectList_act.List[(int)obj_pairs[i][ 1]].Pos3D.f_y;
					pos1[2] = str_RobotObjectList_act.List[(int)obj_pairs[i][ 1]].Pos3D.f_z;
				//	it = str_RobotObjectList.List.find(obj_pairs[i, 0]);
					pos1[0] = str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Pos3D.f_x;
					pos0[1] = str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Pos3D.f_y;
					pos0[2] = str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Pos3D.f_z;

					auxTime = str_RobotObjectList.List[(int)obj_pairs[i][ 0]].r_DetectionTime;
					//Time Spent in seconds
					inc_time = (ActTime.sec - auxTime.sec);

					vx = (pos1[0] - pos0[0]) / inc_time;
					vy = (pos1[1] - pos0[1]) / inc_time;
					vz = (pos1[2] - pos0[2]) / inc_time;

					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Pos3D.f_x = pos1[0];
					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Pos3D.f_y = pos1[1];
					str_RobotObjectList.List[(int)obj_pairs[i][0]].Pos3D.f_z = pos1[2];
					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Vel3D.d_vx = vx;
					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Vel3D.d_vy = vy;
					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].Vel3D.d_vz = vz;

					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].r_DetectionTime = ActTime;

					str_RobotObjectList.List[(int)obj_pairs[i][ 0]].PhysCharac.f_PhysSize = str_RobotObjectList_act.List[(int)obj_pairs[i][ 1]].PhysCharac.f_PhysSize;
					max_id = fmax(max_id, (int)str_RobotObjectList.List[(int)obj_pairs[i][ 0]].f_Identification);

				}
			}
			//Remove objects not seen or actualize position with last known velocity
			int eliminate[n_list];
			n_eliminate = 0;
			for (int i = 0; i < n_list; i++)
			{
				eliminate[i] = 0;
				if (str_RobotObjectList.List[i].r_DetectionTime != ActTime)
				{
					auxTime = str_RobotObjectList.List[i].r_DetectionTime;
					if (auxTime.sec + t_min_detect < ActTime.sec)
					{
						//eliminate this object
						eliminate[i] = 1;
						n_eliminate++;
					}
				}
			}
			//Eliminate the objects
			//Create auxiliary array of structures to save the objects that are not going to be eliminated
			i_aux = 0;
			for (int i = 0; i < n_act; i++)
			{
				if (obj_act_added[i] == 0)
					n_add++;
			}
		//	DynObjects_aux = new cl_DynamicObjects((ushort)(n_list - n_eliminate + n_add));
			cl_DynamicObjects2 DynObjects_aux = cl_DynamicObjects2(n_list-n_eliminate+n_add);
			for (int i = 0; i < n_list; i++)
			{
				if (eliminate[i] == 0)
				{
					DynObjects_aux.List[i_aux] = str_RobotObjectList.List[i_aux];
					i_aux++;
				}
			}
			max_id = fmax(max_id, maxIDrobot);
			//Add the objects that have not been paired
			for (int i = 0; i < n_act; i++)
			{
				if (obj_act_added[i] == 0)
				{
					DynObjects_aux.List[i_aux] = str_RobotObjectList_act.List[i];
					DynObjects_aux.List[i_aux].r_DetectionTime = ActTime;
					DynObjects_aux.List[i_aux].Vel3D.d_vx = 0;
					DynObjects_aux.List[i_aux].Vel3D.d_vy = 0;
					DynObjects_aux.List[i_aux].Vel3D.d_vz = 0;
					DynObjects_aux.List[i_aux].f_Identification = max_id + 1;
//					DynObjects_aux.List[i_aux].Pos3D.f_x = str_RobotObjectList_act.List[i].Pos3D.f_x;
//					DynObjects_aux.List[i_aux].Position.d_y = str_RobotObjectList_act.DynamicObjects.List[i].Position.d_y;
//					DynObjects_aux.List[i_aux].f_DetectionTime = ActTime;
//					DynObjects_aux.List[i_aux].Velocity.d_xVel = 0;
//					DynObjects_aux.List[i_aux].Velocity.d_yVel = 0;
//					DynObjects_aux.List[i_aux].PhysCharac.f_PhysSize = str_RobotObjectList_act.DynamicObjects.List[i].PhysCharac.f_PhysSize;
					//DynObjects_aux.List[i_aux].us_Identification = (ushort)(max_id + 1);
					max_id++;
					i_aux++;
				}
			}
			str_RobotObjectList = DynObjects_aux;
			str_RobotObjectList.maxID = max_id;

		}
		return str_RobotObjectList;
	}


};
}





//public static RobotObjectList ObjectIdentification(RobotObjectList str_RobotObjectList_act,
//                                                           RobotObjectList str_RobotObjectList,
//                                                           float ActTime,
//                                                            char[] RobotID, ushort maxIDrobot)
//        {
//            int n_list = -1;
//            int n_act = -1;
//            int n_add = 0;
//            double[,] pred_pos;
//            double[,] act_pos;
//            float[,] pairs;
//            float[,] obj_pairs;
//            bool first = true;
//            float distance;
//            int i_aux = 0;
//            int max_id = 0;
//            float distance_tol = DynObj_Const.f_object_distance_tol;
//            float t_min_detect = DynObj_Const.t_min_detect;
//            float dist_min = 0;
//            double[] pos1 = new double[2], pos0 = new double[2];
//            double vx, vy;
//            int[] eliminate;
//            int n_eliminate;
//            float inc_time = 0;
//            int[] obj_act_added;
//            float auxTime;
//            cl_DynamicObjects DynObjects_aux = new cl_DynamicObjects();
//            //*Identify objects from DynamicObjectsList.List
//            // * with the ones defined in DynamicObjectsAct
//
//            //Calculate the number of elements in the dynamic object list and in the actual object list
//            n_list = str_RobotObjectList.DynamicObjects.List.Length;
//            n_act = str_RobotObjectList_act.DynamicObjects.List.Length;
//            if (n_list == 0 && n_act>0)
//            {
//                str_RobotObjectList.DynamicObjects = new cl_DynamicObjects((ushort)n_act);
//                str_RobotObjectList.DynamicObjects = str_RobotObjectList_act.DynamicObjects;
//
//            }
//            else if (n_act > 0 && n_list > 0)
//            {
//                n_eliminate = 0;
//
//                pred_pos = new double[n_list, 2];
//                act_pos = new double[n_act, 2];
//                pairs = new float[n_list * n_act, 3]; //ind_pred, ind_act,distance //all the possible pairs
//                obj_pairs = new float[Math.Min(n_list, n_act), 3]; //ind_pred,ind_act,distance //selected pairs
//
//                obj_act_added = new int[n_act];
//                //calculate the distance between the pairs
//                for (int i = 0; i < n_list; i++)
//                {
//                    pos0[0] = str_RobotObjectList.DynamicObjects.List[i].Position.d_x;
//                    pos0[1] = str_RobotObjectList.DynamicObjects.List[i].Position.d_y;
//                    vx = str_RobotObjectList.DynamicObjects.List[i].Velocity.d_xVel;
//                    vy = str_RobotObjectList.DynamicObjects.List[i].Velocity.d_yVel;
//                    auxTime = str_RobotObjectList.DynamicObjects.List[i].f_DetectionTime;
//                    pred_pos[i, 0] = pos0[0] + vx * (ActTime - auxTime);
//                    pred_pos[i, 1] = pos0[1] + vy * (ActTime - auxTime);
//                    for (int j = 0; j < n_act; j++)
//                    {
//                        if (first)
//                        {
//                            obj_act_added[j] = 0; // all the objects marked as not added. later mark the one added with 1
//                            act_pos[j, 0] = (float)str_RobotObjectList_act.DynamicObjects.List[j].Position.d_x;
//                            act_pos[j, 1] = (float)str_RobotObjectList_act.DynamicObjects.List[j].Position.d_y;
//                        }
//                        distance = (float)Math.Sqrt((double)(Math.Pow((double)(act_pos[j, 0] - pred_pos[i, 0]), 2) +
//                                                             Math.Pow((double)(act_pos[j, 1] - pred_pos[i, 1]), 2)));
//                        pairs[j + i * n_act, 0] = i;
//                        pairs[j + i * n_act, 1] = j;
//                        pairs[j + i * n_act, 2] = distance;
//
//                    }
//                    first = false;
//                }
//                //Order and select pairs
//                dist_min = 100000;
//                while (i_aux < Math.Min(n_list, n_act))
//                {
//                    for (int i = 0; i < (n_act * n_list); i++)
//                    {
//                        if (pairs[i, 2] < dist_min && pairs[i,2]>0)
//                        {
//                            obj_pairs[i_aux, 0] = pairs[i, 0];
//                            obj_pairs[i_aux, 1] = pairs[i, 1];
//                            obj_pairs[i_aux, 2] = pairs[i, 2];
//                            dist_min = pairs[i, 2];
//                        }
//                    }
//                    for (int i = 0; i < (n_act * n_list); i++)
//                    {
//                        if (pairs[i, 0] == obj_pairs[i_aux, 0])
//                            pairs[i, 2] = -1;
//                        if (pairs[i, 1] == obj_pairs[i_aux, 1])
//                            pairs[i, 2] = -1;
//                    }
//                    dist_min = 100000;
//                    i_aux++;
//
//                }
//                //Check each pair to see if distance its acceptable and to predict new position
//
//                for (int i = 0; i < Math.Min(n_act, n_list); i++)
//                {
//                    if (obj_pairs[i, 2] < distance_tol)
//                    {
//                        obj_act_added[(int)obj_pairs[i, 1]] = 1;
//                        pos1[0] = str_RobotObjectList_act.DynamicObjects.List[(int)obj_pairs[i, 1]].Position.d_x;
//                        pos1[1] = str_RobotObjectList_act.DynamicObjects.List[(int)obj_pairs[i, 1]].Position.d_y;
//                        pos0[0] = str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_x;
//                        pos0[1] = str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_y;
//
//                        auxTime = str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].f_DetectionTime;
//                        //Time Spent in seconds
//                        inc_time = (ActTime - auxTime);
//
//                        vx = (pos1[0] - pos0[0]) / inc_time;
//                        vy = (pos1[1] - pos0[1]) / inc_time;
//
//                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_x = pos1[0];
//                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Position.d_y = pos1[1];
//                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Velocity.d_xVel = vx;
//                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].Velocity.d_yVel = vy;
//
//                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].f_DetectionTime = ActTime;
//
//                        str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].PhysCharac.f_PhysSize = str_RobotObjectList_act.DynamicObjects.List[(int)obj_pairs[i, 1]].PhysCharac.f_PhysSize;
//                        max_id = Math.Max(max_id, (int)str_RobotObjectList.DynamicObjects.List[(int)obj_pairs[i, 0]].us_Identification);
//
//                    }
//                }
//                //Remove objects not seen or actualize position with last known velocity
//                eliminate = new int[n_list];
//                n_eliminate = 0;
//                for (int i = 0; i < n_list; i++)
//                {
//                    eliminate[i] = 0;
//                    if (str_RobotObjectList.DynamicObjects.List[i].f_DetectionTime != ActTime)
//                    {
//                        auxTime = str_RobotObjectList.DynamicObjects.List[i].f_DetectionTime;
//                        if (auxTime + t_min_detect < ActTime)
//                        {
//                            //eliminate this object
//                            eliminate[i] = 1;
//                            n_eliminate++;
//                        }
//                    }
//                }
//                //Eliminate the objects
//                //Create auxiliary array of structures to save the objects that are not going to be eliminated
//                i_aux = 0;
//                for (int i = 0; i < n_act; i++)
//                {
//                    if (obj_act_added[i] == 0)
//                        n_add++;
//                }
//                DynObjects_aux = new cl_DynamicObjects((ushort)(n_list - n_eliminate + n_add));
//                for (int i = 0; i < n_list; i++)
//                {
//                    if (eliminate[i] == 0)
//                    {
//                        DynObjects_aux.List[i_aux] = str_RobotObjectList.DynamicObjects.List[i_aux];
//                        i_aux++;
//                    }
//                }
//                max_id = Math.Max(max_id, maxIDrobot);
//                //Add the objects that have not been paired
//                for (int i = 0; i < n_act; i++)
//                {
//                    if (obj_act_added[i] == 0)
//                    {
//                        DynObjects_aux.List[i_aux] = new Interfaces.Complex.cl_DynamicObjectList();
//                        DynObjects_aux.List[i_aux].Position.d_x = str_RobotObjectList_act.DynamicObjects.List[i].Position.d_x;
//                        DynObjects_aux.List[i_aux].Position.d_y = str_RobotObjectList_act.DynamicObjects.List[i].Position.d_y;
//                        DynObjects_aux.List[i_aux].f_DetectionTime = ActTime;
//                        DynObjects_aux.List[i_aux].Velocity.d_xVel = 0;
//                        DynObjects_aux.List[i_aux].Velocity.d_yVel = 0;
//                        DynObjects_aux.List[i_aux].PhysCharac.f_PhysSize = str_RobotObjectList_act.DynamicObjects.List[i].PhysCharac.f_PhysSize;
//                        DynObjects_aux.List[i_aux].us_Identification = (ushort)(max_id + 1);
//                        max_id++;
//                        i_aux++;
//                    }
//                }
//                str_RobotObjectList.DynamicObjects = DynObjects_aux;
//                str_RobotObjectList.maxID = (ushort)max_id;
//
//            }
//            return str_RobotObjectList;
//        }
//
//
//    }
//}
